#ifndef MATCHER_H
#define MATCHER_H

#include <eigen3/Eigen/Core>

#include "hungarian_optimizer.h"
#include "object_track.h"
#include "post_process_variable.h"


using TrackObjectPair = std::pair<std::int32_t, std::int32_t>;

class Mathcer  {
   public:
  Mathcer();
  Mathcer(Mathcer const &) = delete;             // 不允许拷贝构造函数
  Mathcer &operator=(Mathcer const &) = delete;  //不允许赋值运算符重载
  Mathcer(Mathcer &&) = delete;                  //不允许左值传递
  Mathcer &operator=(Mathcer &&) = delete;  //不允许重载地址传递的运算符
  ~Mathcer();

  /// @brief main Match function
  /// @param void
  /// @return void
  void Match(std::vector<TrackedObjectPtr> *objects,
             const std::vector<ObjectTrackPtr> &tracks,
             const std::vector<Eigen::VectorXf> &tracks_predict,
             std::vector<std::pair<int, int>> *assignments,
             std::vector<int> *unassigned_tracks,
             std::vector<int> *unassigned_objects);

  /// @brief main Match function
  /// @param void
  /// @return void
  void SetData();

 private:
  /// @brief initialize matcher
  /// @param void
  /// @return void
  void init();

  /// @brief compute association matrix
  /// @param[IN] tracks: maintained tracks for matching
  /// @param[IN] tracks_predict: predicted states of maintained tracks
  /// @param[IN] new_object: recently detected objects
  /// @param[OUT] association_mat: matrix of association distance
  /// @return nothing
  void compute_associate_matrix(
      std::vector<ObjectTrackPtr> const &tracks,
      std::vector<Eigen::VectorXf> const &tracks_predict,
      std::vector<TrackedObjectPtr> const &new_objects,
      Eigen::MatrixXf *association_mat);

  /// @brief compute connection_evaluation between tracks and new detected
  /// objects for association matrix
  float compute_connection_evaluation(ObjectTrackPtr const &track,
                                      Eigen::VectorXf const &track_predict,
                                      TrackedObjectPtr const &new_object);

  /// @brief compute cos theta from xy
  float VectorCosTheta2fXy(Eigen::Vector3f const &v1,
                           Eigen::Vector3f const &v2);
  /// @brief compute connected components within given threshold
  /// @param[IN] association_mat: matrix of association distance
  /// @param[IN] connected_threshold: threshold of connected components
  /// @param[OUT] track_components: connected objects of given tracks
  /// @param[OUT] obj_components: connected tracks of given objects
  /// @return nothing
  void compute_connected_components(
      Eigen::MatrixXf const &association_mat, float const connected_threshold,
      std::vector<std::vector<std::int32_t>> *track_components,
      std::vector<std::vector<std::int32_t>> *object_components);

  /// @brief analysis connected compenent(找连通域)
  /// @param[IN] graph: 2-d vector for connected row and col
  /// @param[OUT] components: 2-d connected compenents
  /// @return nothing
  void connected_component_analysis(const std::vector<std::vector<int>> &graph,
                                    std::vector<std::vector<int>> *components);

  /// @brief match detected objects to tracks in component level
  /// @param[IN] association_mat: association matrix of all objects to tracks
  /// @param[IN] track_component: component of track idx
  /// @param[IN] object_component: component of object idx
  ///@param [IN] connection_threshold : match threshold
  /// @param[OUT] sub_assignments: component assignement pair of object & track
  /// @param[OUT] sub_unassigned_tracks: component tracks not matched
  /// @param[OUT] sub_unassigned_objects: component object not matched
  /// @return nothing
  void match_in_components(Eigen::MatrixXf const &association_mat,
                           std::vector<std::int32_t> const &track_component,
                           std::vector<std::int32_t> const &object_component,
                           float const &connection_threshold,
                           std::vector<TrackObjectPair> *sub_assignments,
                           std::vector<std::int32_t> *sub_unassigned_tracks,
                           std::vector<std::int32_t> *sub_unassigned_objects);

  /// @brief assign objects to tracks using components
  void assign_objects_to_tracks(
      Eigen::MatrixXf const &association_mat,
      const double assign_distance_maximum,
      std::vector<TrackObjectPair> *assignments,
      std::vector<std::int32_t> *unassigned_tracks,
      std::vector<std::int32_t> *unassigned_objects) noexcept;

  /// @brief compute track&new object gIou2d
  float compute_giou2d(ObjectTrackPtr const& track,
                       Eigen::VectorXf const& track_predict,
                       TrackedObjectPtr const& new_object) noexcept;

  ///@brief convert Object to Box
  void object_to_box(ObjectTrackPtr const& track, Box& b) noexcept;
  void object_to_box(TrackedObjectPtr const& track, Box& b) noexcept;

  /// @brief weight factor of location
  static float s_location_distance_weight_;
  /// @brief weight factor for direction
  static float s_direction_distance_weight_;
  /// @brief weight factor for bbox size
  static float s_bbox_size_distance_weight_;
  /// @brief threshold for distance matching
  static float s_match_distance_maximum_;
};



#endif
